// MAVION ORANGE PID GAIN
#define KP_ROLL 3.5
#define KD_ROLL (0.3*KP_ROLL)
#define KI_ROLL 1.

#define KP_PITCH 8.
#define KD_PITCH (0.25*KP_PITCH)
#define KI_PITCH 1.
#define K_PITCHofROLL 0.

#define KP_YAW 0.
//#define KD_YAW (0.2*KP_YAW)
#define KD_YAW 1.
#define KI_YAW 0.

#define KD_COMMAND_PART 0.      // for special KD implementation (between 0 and 1)

//Saturations
#define I_E_SATURATION 5.       //saturation of input of integrators
#define I_S_SATURATION 200      //saturation of output of integrators
#define COMMAND_SATURATION 500  //saturation of output of PIDs

/* ***************************************************************************************** */
#define PRINT_DEBUG 0   //Will print Debug messages
#define PRINT_ANALOGS 0 //Will print the analog raw data
#define PRINT_EULER 0   //Will print the Euler angles Roll, Pitch and Yaw
#define PRINT_RADIO 0   //print rc channels
#define PRINT_MATLAB 0  //print infos for matlab reading
#define PRINT_GCS 1     //print infos for ground station
#define PRINT_BINARY 0  //Will print binary message and suppress ASCII messages (above)

#define SOFTWARE_VER "1.71"

/* ***************************************************************************************** */
// Radio settings
#define MIN_CHANN 1100          // Minimum pulse width
#define MAX_CHANN 1900          // Maximum pulse width
#define NEUTRAL_CHANN 1500      // Neutral value
#define NB_CHANNELS 8           // Number of radio channels

// MX 16
#define CHANN_THROTTLE 1        //channel throttle
#define CHANN_ROLL 2            //channel roll (should be MAX_CHANN for right turn otherwise reverse the channel)
#define CHANN_ELEVATOR 3        //channel elevator (should be MAX_CHANN for pitch up otherwise reverse the channel)
#define CHANN_RUDDER 4          //channel rudder (should be MAX_CHANN for right turn otherwise reverse the channel)
#define CHANN_MODE 5            //channel mode (3-positions switch)
#define CHANN_KNOB1 6           //channel knob1 (to adjust P gains)
#define CHANN_KNOB2 7           //channel knob2 (to adjust D gains)
#define CHANN_SWITCH 8          //channel switch (to tune pitch or roll gains)

//COCKPIT SX
/*#define CHANN_THROTTLE 4        //channel throttle
 #define CHANN_ROLL 1            //channel roll (should be MAX_CHANN for right turn otherwise reverse the channel)
 #define CHANN_ELEVATOR 2        //channel elevator (should be MAX_CHANN for pitch up otherwise reverse the channel)
 #define CHANN_RUDDER 3          //channel rudder (should be MAX_CHANN for right turn otherwise reverse the channel)
 #define CHANN_MODE 7            //channel mode (3-positions switch)
 #define CHANN_KNOB1 6           //channel knob1 (to adjust P gains)
 #define CHANN_KNOB2 6           //channel knob2 (to adjust D gains)
 #define CHANN_SWITCH 6          //channel switch (to tune pitch or roll gains)*/

#define REVERSE_ROLL -          // Reverse roll  (+ or -)
#define REVERSE_PITCH -         // Reverse pitch (+ or -)
#define REVERSE_YAW -           // Reverse yaw (+ or -)
#define REVERSE_THROTTLE +      // Reverse throttle (+ or -)

#define MIX_ROLL 0.25           // Mixing roll  (O to 1)
#define MIX_PITCH 0.5           // Mixing pitch (O to 1)
#define MIX_YAW 0.5             // Mixing yaw (O to 1)
#define MIX_THROTTLE 1          // Mixing throttle (O to 1)

#define FAILSAFE_ROLL 0         // Roll in failsafe mode
#define FAILSAFE_PITCH 0        // Pitch in failsafe mode

#define MAX_ROLL 15.            // Maximum absolute roll (in °)
#define MAX_PITCH 15.           // Maximum absolute pitch (in °)
#define MAX_YAW_SPEED 25.       // Maximum absolute yaw speed (in °/s)
#define CRUISE_SPEED 0          // Cruise speed (in m/s) (for centrifugal force estimation)

// Radio modes
#define MANUAL 0
#define FLY_BY_WIRE 1
#define TEST_MODE 2
#define NO_RADIO 3

/* ***************************************************************************************** */
// Mixing
//motor left 0
//motor right 1
//servo left 2
//servo right 3

#define SERVO_0_COMMAND (control_throttle + control_roll)
#define SERVO_1_COMMAND (control_throttle - control_roll)
#define SERVO_2_COMMAND (control_pitch + control_yaw)
#define SERVO_3_COMMAND (- control_pitch + control_yaw)

//MIN_CHANN for ESC and NEUTRAL_CHANN for servos
#define SERVO_0_NEUTRAL MIN_CHANN
#define SERVO_1_NEUTRAL MIN_CHANN
#define SERVO_2_NEUTRAL NEUTRAL_CHANN
#define SERVO_3_NEUTRAL NEUTRAL_CHANN

#define TRIM_SERVO_0 0          // Trim servo 0 (in us)
#define TRIM_SERVO_1 0          // Trim servo 1 (in us)
#define TRIM_SERVO_2 0          // Trim servo 2 (in us)
#define TRIM_SERVO_3 0          // Trim servo 3 (in us)

#define OUTPUT_MAESTRO 1        // Use Pololu Maestro for servos output
#define OUTPUT_SERVOTIMER2 0    // Use ServoTimer2 for servos output (Not Implemented yet)

//Loop speed
#define AHRS_LOOP 100           // IMU main loop (in Hz)
#define MAGNETOMETER_LOOP 10    // IMU magnetometer loop (in Hz)
#define TELEMETRY_LOOP 5        // Telemetry loop (in Hz)
#define CONTROL_LOOP 50         // Control (PID + servos update) loop (in Hz)


